{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "#!/usr/bin/env python3\n",
    "#coding=utf-8\n",
    "import time\n",
    "from Rosmaster_Lib import Rosmaster\n",
    "from ipywidgets import interact\n",
    "import ipywidgets as widgets\n",
    "import math\n",
    "import pygame #\n",
    "from simple_pid import PID\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "B=110e-3  #С?????? m\n",
    "l=127e-3   #С????? m\n",
    "\n",
    "#???????????????\n",
    "midtheta1=57\n",
    "midtheta2=62\n",
    "midtheta3=119\n",
    "midtheta4=57  \n",
    "\n",
    "x1_old ,x2_old, x3_old ,x4_old=0,0,0,0\n",
    "\n",
    "# ????Rosmaster???? bot Create the Rosmaster object bot\n",
    "bot = Rosmaster()\n",
    "# ???????????? Start receiving data\n",
    "bot.create_receive_threading()\n",
    "\n",
    "\n",
    "\n",
    "# def car_runtype(models):\n",
    "#     if models==0:\n",
    "        \n",
    "#     elif models==1:\n",
    "\n",
    "    \n",
    "    \n",
    "#????????????? -90??~90??\n",
    "def limittheta(theta):\n",
    "    if theta>=90:\n",
    "        theta=90\n",
    "    elif theta<=-90:\n",
    "        theta=-90\n",
    "    else : theta=theta\n",
    "    return theta \n",
    "\n",
    "# ????PWM??? Control PWM steering gear  \n",
    "def pwm_servo(S1, S2, S3, S4):\n",
    "    bot.set_pwm_servo(1, S1)\n",
    "    bot.set_pwm_servo(2, S2)\n",
    "    bot.set_pwm_servo(3, S3)\n",
    "    bot.set_pwm_servo(4, S4)\n",
    "\n",
    "#???????????????\n",
    "def doubleAKM(thetaf):\n",
    "    thetar=-thetaf\n",
    "    Q=B*(math.atan(thetaf)-math.tan(thetar))/(2*l)\n",
    "    thetafl=math.degrees(math.atan(math.tan(thetaf)/(1-Q)))#?????????thetafl\n",
    "    thetafr=math.degrees(math.atan(math.tan(thetaf)/(1+Q)))#?????????thetafr\n",
    "    thetarl=math.degrees(math.atan(math.tan(thetar)/(1-Q)))#?????????thetarl\n",
    "    thetarr=math.degrees(math.atan(math.tan(thetar)/(1+Q)))#?????????thetarr\n",
    "\n",
    "    ##??????????? \n",
    "    get_thetafl=midtheta1+limittheta(thetafl)\n",
    "    get_thetafr=midtheta1+limittheta(thetafr)\n",
    "    get_thetarl=midtheta1+limittheta(thetarl)\n",
    "    get_thetarr=midtheta1+limittheta(thetarr)\n",
    "    pwm_servo(get_thetafl,get_thetafr,get_thetarl,get_thetarr)\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "#??????????\n",
    "#pygame.init()\n",
    "#pygame.joystick.init()\n",
    "\n",
    "# ?????????????????????????????????0\n",
    "#joystick = pygame.joystick.Joystick(0)\n",
    "# ???????????\n",
    "#joystick.init()\n",
    "\n",
    "while True:\n",
    "    # ?????????????\n",
    "\n",
    "\n",
    "    #pygame.event.get()?????????????????????????б?\n",
    "    #for event_ in pygame.event.get():\n",
    "         #pygame.JOYBUTTONDOWN??event_.type == pygame.JOYBUTTONUP?????????????\"????????\"??\"???????\"\n",
    "       # if event_.type == pygame.JOYBUTTONDOWN or event_.type == pygame.JOYBUTTONUP:\n",
    "            #joystick.get_button(3)????????3(Y??)??????????1??????0????\n",
    "    #??????    \n",
    "\n",
    "\n",
    "    \n",
    "    x1_out,x2_out,x3_out,x4_out=bot.get_motor_encoder()\n",
    "\n",
    "    x1_now=x1_out-x1_old\n",
    "    x2_now=x2_out-x2_old\n",
    "    x3_now=x3_out-x3_old\n",
    "    x4_now=x4_out-x4_old\n",
    "    x1_old ,x2_old, x3_old ,x4_old= x1_out,x2_out,x3_out,x4_out\n",
    "\n",
    "    print(\"bianmaqi:fl:%d,fr:%d,rl:%d,rr:%d\\n\",x1_now,x2_now,x3_now,x4_now)\n",
    "\n",
    "\n",
    "\n",
    "    #####PIDconture\n",
    "    PWM1 = PID(0.3, 0, 0.1, setpoint=x1_now)\n",
    "    PWM1.output_limits = (-30,30)\n",
    "    PWM2 = PID(0.3, 0, 0.1, setpoint=x2_now)\n",
    "    PWM2.output_limits = (-30,30)\n",
    "    PWM3 = PID(0.3, 0, 0.1, setpoint=x3_now)\n",
    "    PWM3.output_limits = (-30,30)\n",
    "    PWM4 = PID(0.3, 0, 0.1, setpoint=x4_now)\n",
    "    PWM4.output_limits = (-30,30)\n",
    "\n",
    "    bot.set_motor(PWM1, PWM2, PWM3, PWM4)\n",
    "    time.sleep(0.1)\n",
    "\n",
    "\n",
    "    #user_input = input(\"??????q???: \")\n",
    "    #if user_input == 'q':\n",
    "        #bot.set_motor(0, 0, 0, 0)\n",
    "                # ????????????????????????????????????ROSMASTER???????\n",
    "        # After the program is complete, delete the object to avoid conflicts caused by using the library in other programs\n",
    " \n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "bot.set_motor(0, 0, 0, 0)\n",
    "del bot"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.10"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
